#include <fcntl.h>
#include <linux/joystick.h>

// location of joystick in linux
#define DEFAULT_DEV "/dev/input/js0"

// joystick axis codes
#define XAXIS		0
#define YAXIS		1
#define THROTTLE	2
#define ZAXIS		3

// normalizing macros
#define AXIS_MAX                            32767.0
#define NORMALIZE_SPEED(X)                  ( (X) * (max_speed) / 5 )
#define NORMALIZE_TURN(X)                   ( (X) * (max_turn) /  5 )
#define FACTOR                              (AXIS_MAX - event.value)/(2*AXIS_MAX)
#define ADJUST_SPEED(X)                     (X*FACTOR)
#define ADJUST_TURN(X)                      (DTOR(X)*FACTOR*1)
#define ADJUST_TURN_FACTOR(X)               (1 + X*abs(event.value)/AXIS_MAX)
#define ADJUST_BOUNDS(X,max_old,max_new)    (ceil(X*max_new/max_old))    

double max_speed = 1; // m/second
double max_turn = DTOR(60); // rad/second
double turn_factor = 1;

// INIT joystick interface

int jfd; //symbolic address for joystick
char jsdev[PATH_MAX]; // joystick device file
int button[12] = {0,0,0,0,0,0,0,0,0,0,1,0}; //button states

struct controller{
    
    double speed, turnrate;
    bool dirty; // use this flag to determine when we need to send commands
    
};

// open a joystick device and read from it, scaling the values and
// putting them into the controller struct

void* joystick_handler(void* arg){
    
    // cast to a recognized type
    struct controller* cont = (struct controller*)arg;
    
    struct js_event event;
    
    int buttons_state = 0;
    
    // loop around a joystick read
    for(;;){
        
        // get the next event from the joystick
        read (jfd, &event, sizeof(struct js_event));
        
        if ((event.type & ~JS_EVENT_INIT) == JS_EVENT_BUTTON){
            if (event.value)
                buttons_state |= (1 << event.number);
            else
                buttons_state &= ~(1 << event.number);
        }
        
        // ignore the startup events
        if( event.type & JS_EVENT_INIT){
            continue;
        }
        
        switch( event.type ){
            
            case JS_EVENT_AXIS:{
                
                switch( event.number ){
                    
                    // SIDE-TO-SIDE
                    case XAXIS:
                        if(-event.value == -AXIS_MAX)
                        	cont->turnrate = -5;
                        else if(-AXIS_MAX < -event.value && -event.value < -26000)
                        	cont->turnrate = -4;
                        else if(-26000 <= -event.value && -event.value < -20000)
                        	cont->turnrate = -3;
                        else if(-20000 <= -event.value && -event.value < -10000)
                        	cont->turnrate = -2;
                        else if(-10000 <= -event.value && -event.value < 0)
                        	cont->turnrate = -1;
                        else if(-event.value == 0)
                        	cont->turnrate = 0;
                        else if(0 < -event.value && -event.value <= 10000)
                        	cont->turnrate = 1;
                        else if(10000 < -event.value && -event.value <= 20000)
                        	cont->turnrate = 2;
                        else if(20000 < -event.value && -event.value <= 26000)
                        	cont->turnrate = 3;
                        else if(26000 < -event.value && -event.value < AXIS_MAX)
                        	cont->turnrate = 4;
                        else if(-event.value == AXIS_MAX)
                        	cont->turnrate = 5;
                                               
                        cont->dirty = true;
                        break;
                        
                        // FORWARD-AND-BACK
                    case YAXIS:
                        // set the robot velocity
                        if(button[4])
                            break;
                        if(-event.value == -AXIS_MAX)
                        	cont->speed = -5;
                        else if(-AXIS_MAX < -event.value && -event.value < -26000)
                        	cont->speed = -4;
                        else if(-26000 <= -event.value && -event.value < -20000)
                        	cont->speed = -3;
                        else if(-20000 <= -event.value && -event.value < -10000)
                        	cont->speed = -2;
                        else if(-10000 <= -event.value && -event.value < 0)
                        	cont->speed = -1;
                        else if(-event.value == 0)
                        	cont->speed = 0;
                        else if(0 < -event.value && -event.value <= 10000)
                        	cont->speed = 1;
                        else if(10000 < -event.value && -event.value <= 20000)
                        	cont->speed = 2;
                        else if(20000 < -event.value && -event.value <= 26000)
                        	cont->speed = 3;
                        else if(26000 < -event.value && -event.value < AXIS_MAX)
                        	cont->speed = 4;
                        else if(-event.value == AXIS_MAX)
                        	cont->speed = 5;
                        
                        cont->dirty = true;
                        break;
                        
                    case THROTTLE:
                        
                        max_speed = ADJUST_SPEED(2.0);
                        max_turn = ADJUST_TURN(120);
                        
                        cont->dirty = true;
                        //std::cout << "max_speed= " << max_speed;
                        //std::cout << " max_turn= " << max_turn << std::endl;
                        break;
                        
                    case ZAXIS:
                        
                        turn_factor = ADJUST_TURN_FACTOR(0.5);
                        cont->dirty = true;
                        //std::cout << "turn_factor= " << turn_factor << std::endl;
                        break;
                        
                        default:
                            std::cout << "AXIS " << -(-event.number) << " value " << event.value << std::endl;
                            break;
                }
                
                case JS_EVENT_BUTTON:{
                    
                    if(event.number == 0 && event.value == 1){
                        std::cout << "BUTTON " << (event.number + 1) << " ";
                        if(button[0]==0)
                            button[0] = 1;
                        else
                            button[0] = 0;
                        std::cout << "new state=" << button[0] << std::endl;
                    }
                    else if(event.number == 1 && event.value == 1){
                        std::cout << "BUTTON " << (event.number + 1) << " ";
                        if(button[1]==0)
                            button[1] = 1;
                        else
                            button[1] = 0;
                        std::cout << "new state=" << button[1] << std::endl;
                    }
                    else if(event.number == 2 && event.value == 1){
                        std::cout << "BUTTON " << (event.number + 1) << " ";
                        if(button[2]==0)
                            button[2] = 1;
                        else
                            button[2] = 0;
                        std::cout << "new state=" << button[2] << std::endl;
                    }
                    else if(event.number == 3 && event.value == 1){
                        std::cout << "BUTTON " << (event.number + 1) << " ";
                        if(button[3]==0)
                            button[3] = 1;
                        else
                            button[3] = 0;
                        std::cout << "new state=" << button[3] << std::endl;
                    }
                    else if(event.number == 4 && event.value == 1){
                        std::cout << "BUTTON " << (event.number + 1) << " ";
                        if(button[4]==0)
                            button[4] = 1;
                        else
                            button[4] = 0;
                        std::cout << "new state=" << button[4] << std::endl;
                    }
                    else if(event.number == 5 && event.value == 1){
                        std::cout << "BUTTON " << (event.number + 1) << " ";
                        if(button[5]==0)
                            button[5] = 1;
                        else
                            button[5] = 0;
                        std::cout << "new state=" << button[5] << std::endl;
                    }
                    else if(event.number == 6 && event.value == 1){
                        std::cout << "BUTTON " << (event.number + 1) << " ";
                        if(button[6]==0)
                            button[6] = 1;
                        else
                            button[6] = 0;
                        std::cout << "new state=" << button[6] << std::endl;
                    }
                    else if(event.number == 7 && event.value == 1){
                        std::cout << "BUTTON " << (event.number + 1) << " ";
                        if(button[7]==0)
                            button[7] = 1;
                        else
                            button[7] = 0;
                        std::cout << "new state=" << button[7] << std::endl;
                    }
                    else if(event.number == 8 && event.value == 1){
                        std::cout << "BUTTON " << (event.number + 1) << " ";
                        if(button[8]==0)
                            button[8] = 1;
                        else
                            button[8] = 0;
                        std::cout << "new state=" << button[8] << std::endl;
                    }
                    else if(event.number == 9 && event.value == 1){
                        std::cout << "BUTTON " << (event.number + 1) << " ";
                        if(button[9]==0)
                            button[9] = 1;
                        else
                            button[9] = 0;
                        std::cout << "new state=" << button[9] << std::endl;
                    }
                    else if(event.number == 10 && event.value == 1){
                        //std::cout << "BUTTON " << (event.number + 1) << " ";
                        if(button[10]==0){
                            button[10] = 1;
                            std::cout << "Rear gear locked." << std::endl;
                        }
                        else{
                            button[10] = 0;
                            std::cout << "Rear gear unlocked." << std::endl;
                        }
                        //std::cout << "new state=" << button[10] << std::endl;
                    }
                    else if(event.number == 11 && event.value == 1){
                        //std::cout << "BUTTON " << (event.number + 1) << " ";
                        if(button[11]==0)
                            button[11] = 1;
                        else
                            button[11] = 0;
                        //bstd::cout << "new state=" << button[11] << std::endl;
                    }
                    
                    cont->dirty = true;
                    
                }
                
            }
            
            break;
            
        }
    }
    
    return(NULL);
    
}

struct controller cont;

// END of joystick interface
